/* This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 *    Copyright 2020 (c) Wind River Systems, Inc.
 */

/*
modification history
--------------------
01feb20,lan  written
*/

#include <open62541/plugin/securitypolicy_default.h>
#include <open62541/util.h>

#ifdef UA_ENABLE_ENCRYPTION_OPENSSL

#include <openssl/rsa.h>
#include <openssl/evp.h>
#include <openssl/err.h>
#include <openssl/sha.h>
#include <openssl/x509.h>
#include <openssl/hmac.h>
#include <openssl/aes.h>

#include <open62541/plugin/securitypolicy_openssl_common.h>

#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
#define get_pkey_rsa(evp) EVP_PKEY_get0_RSA(evp)
#else
#define get_pkey_rsa(evp) ((evp)->pkey.rsa)
#endif

#define SHA1_DIGEST_LENGTH 20          /* 160 bits */

/** P_SHA256 Context */
typedef struct UA_Openssl_P_SHA256_Ctx_ {
    size_t  seedLen;    
    size_t  secretLen;
    UA_Byte   A[32]; /* 32 bytes of SHA256 output */
    /*
    char seed[seedLen];
    char secret[secretLen]; */
} UA_Openssl_P_SHA256_Ctx;

#define UA_Openssl_P_SHA256_SEED(ctx)   ((ctx)->A+32)
#define UA_Openssl_P_SHA256_SECRET(ctx) ((ctx)->A+32+(ctx)->seedLen)

/** P_SHA1 Context */
typedef struct UA_Openssl_P_SHA1_Ctx_ {
    size_t  seedLen;    
    size_t  secretLen;
    UA_Byte A[SHA1_DIGEST_LENGTH];  /* 20 bytes of SHA1 output */
    /*
    char seed[seedLen];
    char secret[secretLen]; */
} UA_Openssl_P_SHA1_Ctx;

#define UA_Openssl_P_SHA1_SEED(ctx)   ((ctx)->A + SHA1_DIGEST_LENGTH)
#define UA_Openssl_P_SHA1_SECRET(ctx) ((ctx)->A + SHA1_DIGEST_LENGTH +(ctx)->seedLen)

void 
UA_Openssl_Init (void) {
/* VxWorks7 has initialized the openssl. */        
#ifndef __VXWORKS__  
    static UA_Int16 bInit = 0;
    if (bInit == 1)
        return;
    OpenSSL_add_all_algorithms ();
	ERR_load_crypto_strings ();
    bInit = 1;
#endif    
}

/* UA_copyCertificate - allocalte the buffer, copy the certificate and 
 * add a NULL to the end
 */

UA_StatusCode
UA_copyCertificate (UA_ByteString * dst,
                    const UA_ByteString * src) {
    UA_StatusCode retval = UA_ByteString_allocBuffer (dst, src->length + 1);
    if (retval != UA_STATUSCODE_GOOD)
        return retval;
    (void) memcpy (dst->data, src->data, src->length);
    dst->data[dst->length - 1] = '\0';
    dst->length--;

    return UA_STATUSCODE_GOOD;                        
}

static UA_StatusCode
UA_OpenSSL_RSA_Public_Verify (const UA_ByteString * message,
                              const EVP_MD *        evpMd,
                              X509 *                publicKeyX509,
                              UA_Int16              padding,
                              const UA_ByteString * signature
                              ) {
    EVP_MD_CTX *     mdctx        = NULL;                                  
    int              opensslRet;
    EVP_PKEY_CTX *   evpKeyCtx;  
    EVP_PKEY *       evpPublicKey = NULL;
    UA_StatusCode    ret;

    mdctx = EVP_MD_CTX_create ();
    if (mdctx == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }
    evpPublicKey = X509_get_pubkey (publicKeyX509);
    if (evpPublicKey == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }

    opensslRet = EVP_DigestVerifyInit (mdctx, &evpKeyCtx, evpMd, NULL, 
                                       evpPublicKey);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    EVP_PKEY_CTX_set_rsa_padding (evpKeyCtx, padding);
    opensslRet = EVP_DigestVerifyUpdate (mdctx, message->data, message->length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    opensslRet = EVP_DigestVerifyFinal(mdctx, signature->data, signature->length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }

    ret = UA_STATUSCODE_GOOD;
errout:
    if (evpPublicKey != NULL) {
        EVP_PKEY_free (evpPublicKey);
    }
    if (mdctx != NULL) {
        EVP_MD_CTX_destroy (mdctx);
    }
    return ret;
}

UA_StatusCode
UA_OpenSSL_RSA_PKCS1_V15_SHA256_Verify (const UA_ByteString * msg,
                                        X509 *                publicKeyX509,
                                        const UA_ByteString * signature
                                       ) {
    return UA_OpenSSL_RSA_Public_Verify (msg, EVP_sha256(), publicKeyX509,  
                                         NID_sha256, signature);                                         
}

/* Get certificate thumbprint, and allocate the buffer.
 * 
 */

UA_StatusCode                              
UA_Openssl_X509_GetCertificateThumbprint (const UA_ByteString * certficate,
                                          UA_ByteString *       pThumbprint,
                                          bool                  bThumbPrint) {
    if (bThumbPrint) {
        pThumbprint->length = SHA_DIGEST_LENGTH; 
        UA_StatusCode ret = UA_ByteString_allocBuffer (pThumbprint, pThumbprint->length);
        if (ret != UA_STATUSCODE_GOOD) {
            return ret;
            }
    }
    else {
        if (pThumbprint->length != SHA_DIGEST_LENGTH) {
            return UA_STATUSCODE_BADINTERNALERROR;
        }
    }
    const unsigned char * pTemp = certficate->data;
    X509 * x509Certificate = d2i_X509 (NULL, &pTemp, (long) certficate->length);
    if (x509Certificate == NULL) {
        if (bThumbPrint) {
            UA_ByteString_deleteMembers (pThumbprint);
        }
        return UA_STATUSCODE_BADINTERNALERROR;
    }

    if (X509_digest (x509Certificate, EVP_sha1(), pThumbprint->data, NULL) 
        != 1) {
        if (bThumbPrint) {
            UA_ByteString_deleteMembers (pThumbprint);
        }
    return UA_STATUSCODE_BADINTERNALERROR;
    }
    X509_free(x509Certificate);

    return UA_STATUSCODE_GOOD;                                              
}

static UA_StatusCode
UA_Openssl_RSA_Private_Decrypt (UA_ByteString *       data, 
                               const UA_ByteString * privateKey,
                               UA_Int16              padding) {
    if (data == NULL || privateKey == NULL) {
        return UA_STATUSCODE_BADINVALIDARGUMENT;
    }

    const unsigned char * pkey = privateKey->data;
    EVP_PKEY * evpKey = d2i_PrivateKey(EVP_PKEY_RSA, NULL, 
                        &pkey, (long) privateKey->length);
    if (evpKey == NULL) {
        return UA_STATUSCODE_BADINVALIDARGUMENT;
    }
    
    UA_Int32 keySize = RSA_size(get_pkey_rsa(evpKey));  
    size_t cipherOffset = 0;
    size_t outOffset = 0;
    unsigned char buf[2048];
    UA_Int32 decryptedBytes;

    while (cipherOffset < data->length) {
        decryptedBytes = RSA_private_decrypt (keySize, 
                           data->data + cipherOffset, /* what to decrypt  */
                           buf,                       /* where to decrypt */
                           get_pkey_rsa(evpKey),      /* private key      */
                           padding
                           );
        if (decryptedBytes < 0) {
            EVP_PKEY_free(evpKey);
            return UA_STATUSCODE_BADSECURITYCHECKSFAILED;
        }
        memcpy(data->data + outOffset, buf, (size_t) decryptedBytes);
        cipherOffset += (size_t) keySize;
        outOffset += (size_t) decryptedBytes;
    }
    data->length = outOffset;
    EVP_PKEY_free(evpKey);
    return UA_STATUSCODE_GOOD;
}

UA_StatusCode
UA_Openssl_RSA_Oaep_Decrypt (UA_ByteString *       data, 
                             const UA_ByteString * privateKey) {
    return  UA_Openssl_RSA_Private_Decrypt (data, privateKey, 
                                            RSA_PKCS1_OAEP_PADDING);
}

static UA_StatusCode
UA_Openssl_RSA_Public_Encrypt  (const UA_ByteString * message, 
                                X509 *                publicX509,
                                UA_Int16              padding,
                                size_t                paddingSize,
                                UA_ByteString *       encrypted) {
    EVP_PKEY_CTX *   ctx          = NULL;                                  
    EVP_PKEY *       evpPublicKey = NULL;    
    int              opensslRet;
    UA_StatusCode    ret;

    evpPublicKey = X509_get_pubkey (publicX509);
    if (evpPublicKey == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }
    ctx = EVP_PKEY_CTX_new (evpPublicKey, NULL);
    if (ctx == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;        
    }
    opensslRet = EVP_PKEY_encrypt_init (ctx);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;        
    }
    opensslRet = EVP_PKEY_CTX_set_rsa_padding (ctx, padding);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;        
    }
    
    /* get the encrypted block size */
    size_t encryptedBlockSize;
    RSA *  rsa = get_pkey_rsa (evpPublicKey);
    size_t keySize = (size_t) RSA_size (rsa);
    if (keySize == 0) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    
    switch (padding) {
        case RSA_PKCS1_OAEP_PADDING:
        case RSA_PKCS1_PADDING:
            if (keySize <= paddingSize) {
                ret = UA_STATUSCODE_BADINTERNALERROR;
                goto errout;
            }
            encryptedBlockSize = keySize - paddingSize;
            break;
        default: 
            ret = UA_STATUSCODE_BADNOTSUPPORTED;
            goto errout;
            break;
    }

    /* encrypt in reverse order so that [data] may alias [encrypted] */

    size_t dataPos =  message->length;
    size_t encryptedPos = ((dataPos - 1) / encryptedBlockSize + 1) * keySize;
    size_t bytesToEncrypt = (dataPos - 1) % encryptedBlockSize + 1;
    size_t encryptedTextLen = encryptedPos;
    size_t everyEncryptedBytes;

    while (dataPos > 0) {
        encryptedPos -= keySize;
        dataPos -= bytesToEncrypt;
        opensslRet = EVP_PKEY_encrypt (ctx, encrypted->data + encryptedPos,
             &everyEncryptedBytes, message->data + dataPos, bytesToEncrypt);
       
        if (opensslRet != 1) {
            ret = UA_STATUSCODE_BADINTERNALERROR;
            goto errout;
        }
        bytesToEncrypt = encryptedBlockSize;
    }    
    encrypted->length = encryptedTextLen;

    ret = UA_STATUSCODE_GOOD;
errout:
    if (evpPublicKey != NULL) {
        EVP_PKEY_free (evpPublicKey);
    }
    if (ctx != NULL) {
        EVP_PKEY_CTX_free (ctx);
    }
    return ret;  
}

UA_StatusCode
UA_Openssl_RSA_OAEP_Encrypt (UA_ByteString * data, 
                             size_t          paddingSize,
                             X509 *          publicX509) {
    UA_ByteString message;
    UA_StatusCode ret;
    
    ret = UA_ByteString_copy (data, &message);
    if (ret != UA_STATUSCODE_GOOD) {
        return ret;
    }
    ret = UA_Openssl_RSA_Public_Encrypt (&message, publicX509, 
                                            RSA_PKCS1_OAEP_PADDING, 
                                            paddingSize,
                                            data);
    UA_ByteString_deleteMembers (&message);
    return ret;                                     
}

static UA_Openssl_P_SHA256_Ctx *
P_SHA256_Ctx_Create (const UA_ByteString *  secret,
                     const UA_ByteString *  seed) {
    size_t size = (UA_Int32)sizeof (UA_Openssl_P_SHA256_Ctx) + secret->length +
                    seed->length;
    UA_Openssl_P_SHA256_Ctx * ctx = (UA_Openssl_P_SHA256_Ctx *) UA_malloc (size);
    if (ctx == NULL) {
        return NULL;
    }
    ctx->secretLen = secret->length;
    ctx->seedLen = seed->length;
    (void) memcpy (UA_Openssl_P_SHA256_SEED(ctx), seed->data, seed->length);
    (void) memcpy (UA_Openssl_P_SHA256_SECRET(ctx), secret->data, secret->length);
    /* A(0) = seed
       A(n) = HMAC_HASH(secret, A(n-1)) */
    
    if (HMAC (EVP_sha256(), secret->data, (int) secret->length, seed->data, 
        seed->length, ctx->A, NULL) == NULL) {
        UA_free (ctx);
        return NULL;
    }

    return ctx;
}

static UA_StatusCode 
P_SHA256_Hash_Generate (UA_Openssl_P_SHA256_Ctx * ctx, 
                        UA_Byte *                 pHas
                        ) {
    /* Calculate P_SHA256(n) = HMAC_SHA256(secret, A(n)+seed) */
    if (HMAC (EVP_sha256(),UA_Openssl_P_SHA256_SECRET(ctx), (int) ctx->secretLen,
        ctx->A, sizeof (ctx->A) + ctx->seedLen, pHas, NULL) == NULL) {
            return UA_STATUSCODE_BADINTERNALERROR;
        } 

    /* Calculate A(n) = HMAC_SHA256(secret, A(n-1)) */
   if (HMAC (EVP_sha256(),UA_Openssl_P_SHA256_SECRET(ctx), (int) ctx->secretLen,
        ctx->A, sizeof (ctx->A), ctx->A, NULL) == NULL) {
            return UA_STATUSCODE_BADINTERNALERROR;
        } 
    return UA_STATUSCODE_GOOD;      
}

UA_StatusCode 
UA_Openssl_Random_Key_PSHA256_Derive (const UA_ByteString *     secret,
                                      const UA_ByteString *     seed, 
                                      UA_ByteString *           out) {
    size_t keyLen = out->length;
    size_t iter   = keyLen/32 + ((keyLen%32)?1:0);
    size_t bufferLen = iter * 32;
    size_t i; 
    UA_StatusCode st;

    UA_Byte * pBuffer = (UA_Byte *) UA_malloc (bufferLen);
    if (pBuffer == NULL) {
        return UA_STATUSCODE_BADOUTOFMEMORY;
    }

    UA_Openssl_P_SHA256_Ctx * ctx = P_SHA256_Ctx_Create (secret, seed);
    if (ctx == NULL) {
        UA_free (pBuffer);
        return UA_STATUSCODE_BADOUTOFMEMORY;
    }

    for (i = 0; i < iter; i++) {
        st = P_SHA256_Hash_Generate (ctx, pBuffer + (i * 32));
        if (st != UA_STATUSCODE_GOOD) {
            UA_free (pBuffer);
            UA_free (ctx);
            return st;
        }
    }

    (void) memcpy (out->data, pBuffer, keyLen);
    UA_free (pBuffer);
    UA_free (ctx);
    return UA_STATUSCODE_GOOD;                                          
}

/* return the key bytes */
UA_StatusCode 
UA_Openssl_RSA_Public_GetKeyLength (X509 *     publicKeyX509,
                                    UA_Int32 * keyLen) {
    EVP_PKEY * evpKey = X509_get_pubkey (publicKeyX509);
    if (evpKey == NULL) {
        return  UA_STATUSCODE_BADINTERNALERROR;
    }
    RSA * rsa = get_pkey_rsa (evpKey);
    *keyLen = RSA_size(rsa);
    EVP_PKEY_free (evpKey);
    
    return UA_STATUSCODE_GOOD;
}

UA_StatusCode 
UA_Openssl_RSA_Private_GetKeyLength (const UA_ByteString * privateKey,
                                     UA_Int32 *            keyLen) {
    const unsigned char * pkey = privateKey->data;
    EVP_PKEY * evpKey = d2i_PrivateKey(EVP_PKEY_RSA, NULL, 
                          &pkey, (long) privateKey->length);
    if (evpKey == NULL) {
        return UA_STATUSCODE_BADINVALIDARGUMENT;
    }
    *keyLen = RSA_size(get_pkey_rsa(evpKey));                                           
    EVP_PKEY_free (evpKey);

    return UA_STATUSCODE_GOOD;
}

static UA_StatusCode 
UA_Openssl_RSA_Private_Sign (const UA_ByteString * message,
                     const UA_ByteString * privateKey,
                     const EVP_MD *        evpMd,
                     UA_Int16              padding,                     
                     UA_ByteString *       outSignature) { 
    EVP_MD_CTX *     mdctx        = NULL;                                  
    int              opensslRet;
    EVP_PKEY_CTX *   evpKeyCtx;  
    EVP_PKEY *       evpKey       = NULL;
    UA_StatusCode    ret;

    mdctx = EVP_MD_CTX_create ();
    if (mdctx == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }

    const unsigned char * pkey = privateKey->data;
    evpKey = d2i_PrivateKey(EVP_PKEY_RSA, NULL, 
                        &pkey, (long) privateKey->length);
    if (evpKey == NULL) {
        return UA_STATUSCODE_BADINVALIDARGUMENT;
    }
    opensslRet = EVP_DigestSignInit (mdctx, &evpKeyCtx, evpMd, NULL, evpKey);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    EVP_PKEY_CTX_set_rsa_padding (evpKeyCtx, padding);

    opensslRet = EVP_DigestSignUpdate (mdctx, message->data, message->length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }    
    opensslRet = EVP_DigestSignFinal (mdctx, outSignature->data, &outSignature->length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    } 

    ret = UA_STATUSCODE_GOOD;
errout:
    if (evpKey != NULL) {
        EVP_PKEY_free (evpKey);
    }
    if (mdctx != NULL) {
        EVP_MD_CTX_destroy (mdctx);
    }
    return ret;    
} 

UA_StatusCode 
UA_Openssl_RSA_PKCS1_V15_SHA256_Sign (const UA_ByteString * message,
                                      const UA_ByteString * privateKey,
                                      UA_ByteString *       outSignature) {
    return UA_Openssl_RSA_Private_Sign (message, privateKey, EVP_sha256(), 
                NID_sha256, outSignature); 
}    

UA_StatusCode
UA_OpenSSL_HMAC_SHA256_Verify (const UA_ByteString *     message,
                               const UA_ByteString *     key,
                               const UA_ByteString *     signature
                              ) {
    unsigned char buf[SHA256_DIGEST_LENGTH] = {0};
    UA_ByteString mac = {SHA256_DIGEST_LENGTH, buf};

    if (HMAC (EVP_sha256(), key->data, (int) key->length, message->data, message->length, 
              mac.data, (unsigned int *) &mac.length) == NULL) {
        return UA_STATUSCODE_BADINTERNALERROR;
    }
    if (UA_ByteString_equal (signature, &mac)) {
        return UA_STATUSCODE_GOOD;     
    }
    else {
        return UA_STATUSCODE_BADINTERNALERROR;
    }                  
}

UA_StatusCode
UA_OpenSSL_HMAC_SHA256_Sign (const UA_ByteString *     message,
                             const UA_ByteString *     key,
                             UA_ByteString *           signature
                             ) {
    if (HMAC (EVP_sha256(), key->data, (int) key->length, message->data, 
              message->length,
              signature->data, (unsigned int *) &(signature->length)) == NULL) {
        return UA_STATUSCODE_BADINTERNALERROR;
    }
    return UA_STATUSCODE_GOOD;
} 

static UA_StatusCode
UA_OpenSSL_Decrypt (const UA_ByteString * iv,
                    const UA_ByteString * key,
                    const EVP_CIPHER *    cipherAlg,
                    UA_ByteString *       data  /* [in/out]*/) {
    UA_ByteString    ivCopy    = {0, NULL};
    UA_ByteString    cipherTxt = {0, NULL};
    EVP_CIPHER_CTX * ctx       = NULL;
    UA_StatusCode    ret;  
    int              opensslRet;
    int              outLen;
    int              tmpLen;

    /* copy the IV because the AES_cbc_encrypt function overwrites it. */

    ret = UA_ByteString_copy (iv, &ivCopy);
    if (ret != UA_STATUSCODE_GOOD) {
        goto errout;
    }

    ret = UA_ByteString_copy (data, &cipherTxt);
    if (ret != UA_STATUSCODE_GOOD) {
        goto errout;
    }
    
    ctx = EVP_CIPHER_CTX_new ();
    if (ctx == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }                                
   
    /* call EVP_* to decrypt */
    
    opensslRet = EVP_DecryptInit_ex (ctx, cipherAlg, NULL, key->data, ivCopy.data);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    /* EVP_DecryptFinal() will return an error code if padding is enabled 
     * and the final block is not correctly formatted.  
     */
    EVP_CIPHER_CTX_set_padding (ctx, 0); 
    opensslRet = EVP_DecryptUpdate (ctx, data->data, &outLen, 
                                    cipherTxt.data, (int) cipherTxt.length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }   
    opensslRet = EVP_DecryptFinal_ex (ctx, data->data + outLen, &tmpLen);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    outLen += tmpLen;
    data->length = (size_t) outLen;
    ret = UA_STATUSCODE_GOOD;

errout:
    UA_ByteString_deleteMembers (&ivCopy);
    UA_ByteString_deleteMembers (&cipherTxt);  
    if (ctx != NULL) {
        EVP_CIPHER_CTX_free(ctx);
    }
    return ret;   
}

static UA_StatusCode
UA_OpenSSL_Encrypt (const UA_ByteString * iv,
                    const UA_ByteString * key, 
                    const EVP_CIPHER *    cipherAlg,
                    UA_ByteString *       data  /* [in/out]*/
                    ) {

    UA_ByteString    ivCopy   = {0, NULL};
    UA_ByteString    plainTxt = {0, NULL};
    EVP_CIPHER_CTX * ctx      = NULL;
    UA_StatusCode    ret;  
    int              opensslRet;
    int              outLen;
    int              tmpLen;

    /* copy the IV because the AES_cbc_encrypt function overwrites it. */

    ret = UA_ByteString_copy (iv, &ivCopy);
    if (ret != UA_STATUSCODE_GOOD) {
        goto errout;
    }

    ret = UA_ByteString_copy (data, &plainTxt);
    if (ret != UA_STATUSCODE_GOOD) {
        goto errout;
    }
    
    ctx = EVP_CIPHER_CTX_new ();
    if (ctx == NULL) {
        ret = UA_STATUSCODE_BADOUTOFMEMORY;
        goto errout;
    }

    /* call EVP_* to encrypt */

    opensslRet = EVP_EncryptInit_ex (ctx, cipherAlg, NULL, key->data, ivCopy.data);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    opensslRet = EVP_EncryptUpdate (ctx, data->data, &outLen, 
                                    plainTxt.data, (int) plainTxt.length);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    /* 
     * Buffer passed to EVP_EncryptFinal() must be after data just
     * encrypted to avoid overwriting it.
     */
    opensslRet = EVP_EncryptFinal_ex(ctx, data->data + outLen, &tmpLen);
    if (opensslRet != 1) {
        ret = UA_STATUSCODE_BADINTERNALERROR;
        goto errout;
    }
    outLen += tmpLen;
    data->length = (size_t) outLen;
    ret = UA_STATUSCODE_GOOD;

errout:
    UA_ByteString_deleteMembers (&ivCopy);
    UA_ByteString_deleteMembers (&plainTxt);  
    if (ctx != NULL) {
        EVP_CIPHER_CTX_free(ctx);
    }
    return ret;
}

UA_StatusCode
UA_OpenSSL_AES_256_CBC_Decrypt (const UA_ByteString * iv,
                                const UA_ByteString * key, 
                                UA_ByteString *       data  /* [in/out]*/
                                ) {
    return UA_OpenSSL_Decrypt (iv, key, EVP_aes_256_cbc (), data);                                
}

UA_StatusCode
UA_OpenSSL_AES_256_CBC_Encrypt (const UA_ByteString * iv,
                            const UA_ByteString * key, 
                            UA_ByteString *       data  /* [in/out]*/
                            ) {
    return UA_OpenSSL_Encrypt (iv, key, EVP_aes_256_cbc (), data);
}

UA_StatusCode 
UA_OpenSSL_X509_compare (const UA_ByteString * cert, 
                         const X509 *          bcert) {

    const unsigned char * pData = cert->data;    
    X509 * acert = d2i_X509 (NULL, &pData, 
                             (long) cert->length);
    if (acert == NULL) {
        return UA_STATUSCODE_BADCERTIFICATEINVALID;
    }
    int opensslRet = X509_cmp (acert, bcert);
    X509_free (acert);

    if (opensslRet == 0)
        return UA_STATUSCODE_GOOD;
    return UA_STATUSCODE_UNCERTAINSUBNORMAL;
}

UA_StatusCode
UA_OpenSSL_RSA_PKCS1_V15_SHA1_Verify (const UA_ByteString * msg,
                                      X509 *                publicKeyX509,
                                      const UA_ByteString * signature) {
    return UA_OpenSSL_RSA_Public_Verify (msg, EVP_sha1(), publicKeyX509,  
                                         NID_sha1, signature);                                         
}

UA_StatusCode 
UA_Openssl_RSA_PKCS1_V15_SHA1_Sign (const UA_ByteString * message,
                                    const UA_ByteString * privateKey,
                                    UA_ByteString *       outSignature) {
    return UA_Openssl_RSA_Private_Sign (message, privateKey, EVP_sha1(), 
                                        NID_sha1, outSignature); 
}

static UA_Openssl_P_SHA1_Ctx *
P_SHA1_Ctx_Create (const UA_ByteString *  secret,
                   const UA_ByteString *  seed) {
    size_t size = (UA_Int32)sizeof (UA_Openssl_P_SHA1_Ctx) + secret->length +
                    seed->length;
    UA_Openssl_P_SHA1_Ctx * ctx = (UA_Openssl_P_SHA1_Ctx *) UA_malloc (size);
    if (ctx == NULL) {
        return NULL;
    }

    ctx->secretLen = secret->length;
    ctx->seedLen = seed->length;
    (void) memcpy (UA_Openssl_P_SHA1_SEED(ctx), seed->data, seed->length);
    (void) memcpy (UA_Openssl_P_SHA1_SECRET(ctx), secret->data, secret->length);
    /* A(0) = seed
       A(n) = HMAC_HASH(secret, A(n-1)) */
    
    if (HMAC (EVP_sha1(), secret->data, (int) secret->length, seed->data, 
        seed->length, ctx->A, NULL) == NULL) {
        UA_free (ctx);
        return NULL;
    }

    return ctx;
}

static UA_StatusCode 
P_SHA1_Hash_Generate (UA_Openssl_P_SHA1_Ctx * ctx, 
                      UA_Byte *               pHas
                      ) {
    /* Calculate P_SHA1(n) = HMAC_SHA1(secret, A(n)+seed) */
    if (HMAC (EVP_sha1 (), UA_Openssl_P_SHA1_SECRET(ctx), (int) ctx->secretLen,
        ctx->A, sizeof (ctx->A) + ctx->seedLen, pHas, NULL) == NULL) {
            return UA_STATUSCODE_BADINTERNALERROR;
        } 

    /* Calculate A(n) = HMAC_SHA1(secret, A(n-1)) */
   if (HMAC (EVP_sha1(), UA_Openssl_P_SHA1_SECRET(ctx), (int) ctx->secretLen,
        ctx->A, sizeof (ctx->A), ctx->A, NULL) == NULL) {
            return UA_STATUSCODE_BADINTERNALERROR;
        } 
    return UA_STATUSCODE_GOOD;      
}

UA_StatusCode 
UA_Openssl_Random_Key_PSHA1_Derive (const UA_ByteString *     secret,
                                   const UA_ByteString *     seed, 
                                   UA_ByteString *           out) {
    size_t keyLen     = out->length;
    size_t iter       = keyLen / SHA1_DIGEST_LENGTH + ((keyLen % SHA1_DIGEST_LENGTH)?1:0);
    size_t bufferLen  = iter * SHA1_DIGEST_LENGTH;
    UA_Byte * pBuffer = (UA_Byte *) UA_malloc (bufferLen);
    if (pBuffer == NULL) {
        return UA_STATUSCODE_BADOUTOFMEMORY;
    }

    UA_Openssl_P_SHA1_Ctx * ctx = P_SHA1_Ctx_Create (secret, seed);
    if (ctx == NULL) {
        UA_free (pBuffer);
        return UA_STATUSCODE_BADOUTOFMEMORY;
    }
    
    size_t i; 
    UA_StatusCode st;

    for (i = 0; i < iter; i++) {
        st = P_SHA1_Hash_Generate (ctx, pBuffer + (i * SHA1_DIGEST_LENGTH));
        if (st != UA_STATUSCODE_GOOD) {
            UA_free (pBuffer);
            UA_free (ctx);
            return st;
        }
    }

    (void) memcpy (out->data, pBuffer, keyLen);
    UA_free (pBuffer);
    UA_free (ctx);
    
    return UA_STATUSCODE_GOOD;                                         
}

UA_StatusCode
UA_OpenSSL_HMAC_SHA1_Verify (const UA_ByteString *     message,
                             const UA_ByteString *     key,
                             const UA_ByteString *     signature
                             ) {
    unsigned char buf[SHA1_DIGEST_LENGTH] = {0};
    UA_ByteString mac = {SHA1_DIGEST_LENGTH, buf};

    if (HMAC (EVP_sha1(), key->data, (int) key->length, message->data, message->length, 
              mac.data, (unsigned int *) &mac.length) == NULL) {
        return UA_STATUSCODE_BADINTERNALERROR;
    }
    if (UA_ByteString_equal (signature, &mac)) {
        return UA_STATUSCODE_GOOD;     
    }
    else {
        return UA_STATUSCODE_BADINTERNALERROR;
    }                  
}

UA_StatusCode
UA_OpenSSL_HMAC_SHA1_Sign (const UA_ByteString *     message,
                           const UA_ByteString *     key,
                           UA_ByteString *           signature
                           ) {
    if (HMAC (EVP_sha1(), key->data, (int) key->length, message->data, 
              message->length,
              signature->data, (unsigned int *) &(signature->length)) == NULL) {
        return UA_STATUSCODE_BADINTERNALERROR;
    }
    return UA_STATUSCODE_GOOD;
} 

UA_StatusCode
UA_Openssl_RSA_PKCS1_V15_Decrypt (UA_ByteString *       data, 
                                  const UA_ByteString * privateKey) {
    return  UA_Openssl_RSA_Private_Decrypt (data, privateKey, 
                                            RSA_PKCS1_PADDING);                                      
}

UA_StatusCode
UA_Openssl_RSA_PKCS1_V15_Encrypt (UA_ByteString * data, 
                                  size_t          paddingSize,
                                  X509 *          publicX509) {
    UA_ByteString message;
    UA_StatusCode ret;
    
    ret = UA_ByteString_copy (data, &message);
    if (ret != UA_STATUSCODE_GOOD) {
        return ret;
    }
    ret = UA_Openssl_RSA_Public_Encrypt (&message, publicX509, 
                                         RSA_PKCS1_PADDING, 
                                         paddingSize,
                                         data);
    UA_ByteString_deleteMembers (&message);
    return ret; 
}

UA_StatusCode
UA_OpenSSL_AES_128_CBC_Decrypt (const UA_ByteString * iv,
                                const UA_ByteString * key, 
                                UA_ByteString *       data  /* [in/out]*/
                                ) {
    return UA_OpenSSL_Decrypt (iv, key, EVP_aes_128_cbc (), data);                                
}

UA_StatusCode
UA_OpenSSL_AES_128_CBC_Encrypt (const UA_ByteString * iv,
                                const UA_ByteString * key, 
                                UA_ByteString *       data  /* [in/out]*/
                                ) {
    return UA_OpenSSL_Encrypt (iv, key, EVP_aes_128_cbc (), data);
}

#endif
